package org.maxgroundstation.simulation.service.geometry;

import java.text.DecimalFormat;
import java.util.Iterator;
import java.util.concurrent.ScheduledThreadPoolExecutor;
import java.util.concurrent.TimeUnit;

import org.maxgroundstation.simulation.MaxRobot;
import org.maxgroundstation.simulation.service.Service;
import org.maxgroundstation.simulation.service.ServiceCommunication;
import org.maxgroundstation.simulation.service.ServiceDataFrame;
import org.maxgroundstation.simulation.service.ServiceSubscriber;
import org.maxgroundstation.simulation.service.ServiceUtil;

/**
 * The rover geometry service.
 */
public class GeometryService extends Service {

    // Static members
    public static final String SERVICE_ID = ServiceUtil.getServiceID("geometry");
    public static final long SLEEP_TIME = 100L;
    
    // Data members
    private ScheduledThreadPoolExecutor executor;
    
    /**
     * Constructor
     * @param comm the service communication.
     */
    public GeometryService(ServiceCommunication comm) {
        // Use Service constructor.
        super(comm);
        
        name = "Rover Geometry";
        description = "Displays the rover's actuator and motor values";
        
        // Start update thread execution.
        startExecution();
    }
    
    /**
     * Starts executing the geometry updates.
     */
    private void startExecution() {
        if (executor != null) executor.shutdownNow();
        
        executor = new ScheduledThreadPoolExecutor(1);
        executor.scheduleAtFixedRate(new Runnable() {
            @Override
            public void run() {
                // Update geometry.
                String geometryString = getUpdatedGeometry();
                
                try {
                    sendGeometryString(geometryString);
                }
                catch (Exception e) {
                    comm.log(e);
                }
            }       
        }, 0, SLEEP_TIME, TimeUnit.MILLISECONDS);
    }
    
    /**
     * Gets the updated geometry string.
     * @return geometry string.
     */
    private String getUpdatedGeometry() {
        MaxRobot robot = getServiceCommunication().getRobot();
        DecimalFormat formatter = new DecimalFormat("0.0");
        StringBuffer buff = new StringBuffer();
        
        // Add motor value.
        buff.append("motor=");
        buff.append(formatter.format(robot.getFrontAxleMotor()));
        buff.append(",");
        
        // Add front wheel angle.
        buff.append("front_angle=");
        buff.append(formatter.format(robot.getFrontWheelsTurnAngle()));
        buff.append(",");
        
        // Add rear wheel angle.
        buff.append("rear_angle=");
        buff.append(formatter.format(robot.getRearWheelsTurnAngle()));
        buff.append(",");
        
        // Add pan angle.
        buff.append("pan=");
        buff.append(formatter.format(robot.getHeadPanAngle()));
        buff.append(",");
        
        // Add tilt angle.
        buff.append("tilt=");
        buff.append(formatter.format(robot.getHeadTiltAngle()));
        
        return buff.toString();
    }
    
    /**
     * Sends the geometry string to service subscribers.
     * @param geometryString the geometry string.
     * @throws Exception if error sending geometry string.
     */
    private void sendGeometryString(String geometryString) throws Exception {
        Iterator<ServiceSubscriber> i = subscribers.iterator();
        while (i.hasNext()) {
            ServiceSubscriber subscriber = i.next();
            if (comm.getClientManager().checkClient(subscriber.getClientID())) {
                try {
                    //System.out.println("Sending geometry string: '" + geometryString + "'");
                    comm.sendServicePost(subscriber.getCommType(), getID(), 
                            subscriber.getClientID(), geometryString.getBytes());
                }
                catch (Exception e) {
                    //e.printStackTrace(System.err);
                    comm.log(e);
                }
            }
            else i.remove();
        }
    }
    
    @Override
    public String getID() {
        return SERVICE_ID;
    }

    @Override
    public void handleResponse(ServiceDataFrame dataFrame, int commType) throws Exception {
        // Do nothing.
    }

    @Override
    public boolean isDefaultService() {
        return false;
    }

    @Override
    public boolean isSubscribable() {
        return true;
    }

    @Override
    public void shutDown() {
        executor.shutdownNow();
    }
}